GyroTools.java

  private int spoofedZero(int zeroDegree) {
        int ActualDegree = gyro.getHeading();
        int degree = ActualDegree - zeroDegree;
        if (degree > 360) {
            degree = degree - 360;
        }
        if (degree < 0) {
            degree = degree + 360;
        }
        return degree;
    }
 /*
    DriveOnHeading
    Example of usage:
    rightMotor.power(driveOnHeading(90, 1)[1]);
    In english: Power the right motor with the right power of the function on 90 degrees at 100% power
    leftMotor.power(driveOnHeading(75)[0]);
    In english: Power the left motor with the left power of the function on 75 degree at default of 100% power
     */
    private double[] driveOnHeading(int desiredDegree, double power) {
        double[] returnValues = new double[2];
        int gyroDegree = spoofedZero(desiredDegree);
        int targetDegrees = 0;
        double leftStartPower = power;
        double rightStartPower = power;
        // Experiment with this number \/
        double dividerNumber = 12.5;

        if (gyroDegree > 0 && gyroDegree <= 90) {
            int error_degrees = Math.abs(targetDegrees - gyroDegree);
            double subtractivePower = error_degrees / dividerNumber;
            DbgLog.msg(String.valueOf(subtractivePower + ", " + error_degrees));
            if (power > 0) {
                leftStartPower = Range.clip(1 - subtractivePower, -1, 1);
            } if (power < 0) {
                leftStartPower = Range.clip(1 + subtractivePower, -1, 1);
            }

        }

        if (gyroDegree >= 270 && gyroDegree < 360) {
            int error_degrees = Math.abs(90 - (gyroDegree - 270));
            double subtractivePower = error_degrees / dividerNumber;
            DbgLog.msg(String.valueOf(subtractivePower + ", " + error_degrees));
            if (power > 0) {
                rightStartPower = Range.clip(1 - subtractivePower, -1, 1);
            } if (power < 0) {
                rightStartPower = Range.clip(1 + subtractivePower, -1, 1);
            }

        }

        returnValues[0] = leftStartPower;
        returnValues[1] = rightStartPower;

        return returnValues;
    }

    private double[] driveOnHeading(int desiredDegree) {
        return driveOnHeading(desiredDegree, 1);
    }