MecanumFieldCentricOther

package org.firstinspires.ftc.teamcode.Teleop;

import com.qualcomm.ftccommon.configuration.RobotConfigFile;
import com.qualcomm.hardware.adafruit.AdafruitBNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.robot.Robot;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

import java.util.Arrays;

@TeleOp(name = "FieldCentricDrive")
public class FieldCentricDrive extends OpMode {

    private double initAngleR = 181;
    private double toAngleR = 56;
    private double initAngleL = 64;
    private double toAngleL = 180;

    public Orientation angles;

    double drive;
    double rotate;
    double strafe;

    public ElapsedTime timer1 = new ElapsedTime();

    DcMotor frontLeft;
    DcMotor frontRight;
    DcMotor backLeft;
    DcMotor backRight;

    ElapsedTime eTime;

    private AdafruitBNO055IMU imu;
    boolean initial = true;

    long setTime = System.currentTimeMillis();
    boolean hasRun = false;

    public void delay(double seconds){
        eTime.reset();
        while (eTime.time() < seconds);
    }


    @Override
    public void init() {
        final AdafruitBNO055IMU.Parameters parameters = new AdafruitBNO055IMU.Parameters();
        parameters.angleUnit = AdafruitBNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit = AdafruitBNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        this.imu = this.hardwareMap.get(AdafruitBNO055IMU.class, "bno");
        imu.initialize(parameters);

        angles = this.imu.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZYX);

        frontLeft = hardwareMap.dcMotor.get("FL");
        frontRight = hardwareMap.dcMotor.get("FR");
        backLeft = hardwareMap.dcMotor.get("BL");
        backRight = hardwareMap.dcMotor.get("BR");

        eTime = new ElapsedTime();
    }

    @Override
    public void loop() {
        angles = this.imu.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZYX);

        fieldOriented(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, angles.firstAngle );
        telemetry.addData("Gyro ", (angles.firstAngle));
        telemetry.addData("FL", frontLeft.getPower());
        telemetry.addData("FR", frontRight.getPower());
        telemetry.addData("BL", backLeft.getPower());
        telemetry.addData("BR", backRight.getPower());

        delay(0.05);
    }

    /**
     * @param y
     * @param x
     * @param c
     */

    public void moveBy(double y, double x, double c) {  //joystick values
        double FLval = y + x + c;
        double FRval = y - x - c;
        double BLval = y - x + c;
        double BRval = y + x - c;

        final double DRIVE_REDUC = 1.0;

        angles = this.imu.getAngularOrientation().toAxesReference(AxesReference.INTRINSIC).toAxesOrder(AxesOrder.ZYX);

        //Move range to between 0 and +1, if not already
        double[] wheelPowers = {FRval, FLval, BLval, BRval};
        Arrays.sort(wheelPowers);
        if (wheelPowers[3] > 1) {
            FLval /= wheelPowers[3];  // FL = WheelPowers/FL
            FRval /= wheelPowers[3];
            BLval /= wheelPowers[3];
            BRval /= wheelPowers[3];
        }

        frontLeft.setPower(Range.clip((FLval / DRIVE_REDUC), -1, 1));
        frontRight.setPower(Range.clip((FRval / DRIVE_REDUC), -1, 1));
        backLeft.setPower(Range.clip((BLval / DRIVE_REDUC), -1, 1));
        backRight.setPower(Range.clip((BRval / DRIVE_REDUC), -1, 1));
    }

    /**
     * @param y
     * @param x
     * @param c
     * @param gyroheading
     */

    public void fieldOriented(double y, double x, double c, double gyroheading) {
        double cosA = Math.cos(Math.toRadians(gyroheading));
        double sinA = Math.sin(Math.toRadians(gyroheading));
        double xOut = x * cosA - y * sinA;
        double yOut = x * sinA + y * cosA;
        moveBy(yOut, xOut, c);
    }
}