Field Centric Code

        robot.updateGyro();
        //Changes the angle of the robot's motion with respect to the driver to the robot's current
        //heading when the Y button is pressed
        if(gamepad1.y)
            angleFromDriver = robot.heading;
        //This is the angle that the right joystick is pointing in
        double inputAngle = Math.atan2(-gamepad1.right_stick_y, gamepad1.right_stick_x);
        //This is the magnitude of how far the joystick is pushed
        double inputPower = Math.sqrt(gamepad1.right_stick_x * gamepad1.right_stick_x + gamepad1.right_stick_y *
        gamepad1.right_stick_y);
        telemetry.addData("Joystick Magnitude", inputPower);
        if (inputPower > 1)
            inputPower = 1;
        //This is the angle at which the robot should translate
        double moveAngle = inputAngle + (angleFromDriver - robot.heading);
        //Drives the robot in a field centric fashion, ordering frontLeft,frontRight,backLeft,backRight
        robot.drive(
                (Math.sin(moveAngle) + Math.cos(moveAngle)) * inputPower * robot.currentDrivePower + (gamepad1.left_stick_x) *
                robot.currentDrivePower,
                (Math.sin(moveAngle) - Math.cos(moveAngle)) * inputPower * robot.currentDrivePower + (- gamepad1.left_stick_x) *
                robot.currentDrivePower,
                (Math.sin(moveAngle) - Math.cos(moveAngle)) * inputPower * robot.currentDrivePower + (gamepad1.left_stick_x) *
                robot.currentDrivePower,
                (Math.sin(moveAngle) + Math.cos(moveAngle)) * inputPower * robot.currentDrivePower + (- gamepad1.left_stick_x) *
                robot.currentDrivePower
        );