Issues Turning Using IMU

[KernelPanic]

You can definitely make these calculations smaller and simpler. The remainder and sign operations are very handy.

[FONT=courier new]    double delta = (tarH - currH + 540) % 360 - 180; //gets delta in the range -180 to 180[/FONT]

Nice! Will play around with that. Thanks!

[CoachZM]

Thanks GearTicks abd KernelPanic for the help! Using some of the code we had and combining it with bits and pieces of what you suggested we have a robot that is now accurately turning using the IMU. Thanks again for all of the help!

[GearTicks]

Post 4, this thread. This is about as tidy as I can make it, although it can be done a little smaller.
You wouldn't have to put it in a separate method but could put it in a "while loop" (like "while (returnPwr > 0) { " and instead of using "returnPwr" as a return variable you could set your right side to returnPwr and left side motors to (-1*returnPwr).

You need to pass/have available these four inputs:

1. currH : current heading from the gyro
2. newH: the heading you desire
3. pwrSet: how much "oomph" you want to use for the turn. Positive numbers for clockwise, negative for counterclockwise.
4. turnTime: how long you let this go before you abandon it as stuck. Prevents it from going in circles if the gyro bonks. The sample code doesn't use it, but would be trivial to add in.

[FONT=courier new]public float gyroTurn5(int currH, int newH, float pwrSet, float turnTime)
{[/FONT]
[FONT=courier new]float returnPwr;
[B]int cw = (pwrSet < 0)? -1: 1; // [1] [/B]
[B]int transit = (((currH > newH) && (cw > 0)) || [/B][/FONT]

[FONT=courier new][B]((currH < newH) && (cw < 0))) ? 360 : 0; // [2][/B][/FONT]

[FONT=courier new][B]int desiredRotation = Math.abs(transit + (cw*newH) + ((-1*cw)*currH)); //[3]
desiredRotation = (desiredRotation > 360) ? desiredRotation - 360 : desiredRotation; //[4][/B]
returnPwr = (desiredRotation > 0)? pwrSet : 0;
return returnPwr;[/FONT]
[FONT=courier new]}[/FONT]

This is the guts of the whole thing and it does take a little bit to decipher, but at least it's tight:
desiredRotation = Math.abs(transit + (cw*newH) + ((-1*cw)*currH));

There's a version of it you can run in a regular IDE (like Eclipse or BlueJ) at

https://github.com/50N40W/javacode/blob/master/SampleHeading

You can definitely make these calculations smaller and simpler. The remainder and sign operations are very handy.

public double gyroTurn(double currH, double tarH, double power) {
    double delta = (tarH - currH + 540) % 360 - 180; //gets delta in the range -180 to 180
    return Math.signum(delta) == Math.signum(power) ? power : 0; //may need to add a minus sign to Math.signum(power) if clockwise turns are negative power
}

[KernelPanic]

Does anyone have a (simple) set of code for turning to a heading? We don't need PID and actually only to turn to a value greater/less than a particular heading.

Post 4, this thread. This is about as tidy as I can make it, although it can be done a little smaller.
You wouldn't have to put it in a separate method but could put it in a "while loop" (like "while (returnPwr > 0) { " and instead of using "returnPwr" as a return variable you could set your right side to returnPwr and left side motors to (-1*returnPwr).

You need to pass/have available these four inputs:

1. currH : current heading from the gyro
2. newH: the heading you desire
3. pwrSet: how much "oomph" you want to use for the turn. Positive numbers for clockwise, negative for counterclockwise.
4. turnTime: how long you let this go before you abandon it as stuck. Prevents it from going in circles if the gyro bonks. The sample code doesn't use it, but would be trivial to add in.

[FONT=courier new]public float gyroTurn5(int currH, int newH, float pwrSet, float turnTime)
{[/FONT]
[FONT=courier new]float returnPwr;
[B]int cw = (pwrSet < 0)? -1: 1; // [1] [/B]
[B]int transit = (((currH > newH) && (cw > 0)) || [/B][/FONT]

[FONT=courier new][B]((currH < newH) && (cw < 0))) ? 360 : 0; // [2][/B][/FONT]

[FONT=courier new][B]int desiredRotation = Math.abs(transit + (cw*newH) + ((-1*cw)*currH)); //[3]
desiredRotation = (desiredRotation > 360) ? desiredRotation - 360 : desiredRotation; //[4][/B]
returnPwr = (desiredRotation > 0)? pwrSet : 0;
return returnPwr;[/FONT]
[FONT=courier new]}[/FONT]

This is the guts of the whole thing and it does take a little bit to decipher, but at least it's tight:
desiredRotation = Math.abs(transit + (cw*newH) + ((-1*cw)*currH));

There's a version of it you can run in a regular IDE (like Eclipse or BlueJ) at

https://github.com/50N40W/javacode/blob/master/SampleHeading

[Jynx]

Does anyone have a (simple) set of code for turning to a heading? We don't need PID and actually only to turn to a value greater/less than a particular heading.

[mikets]

If I understand the code correctly, it is basically doing a proportional-only PID control where addSpeed is the Kp constant, delta is error, gyroRange is tolerance. Here is a version using the PID terms in case somebody would understand this easier with the PID terms.

private int turnSettlingCount;

public int gyroCorrect(double target, double tolerance, double minPower, double kP)
{
    double error = (target - getRelativeHeading() + 360.0) % 360.0;
    if (error > 180.0) error -= 360.0;
    if (Math.abs(error) > tolerance)
    {
        turnSettlingCount = 0;
        turn(Math.signum(error)*minPower + kP*error);
    }
    else
    {
        turnSettlingCount++;
        turn(0.0);
    }
    return turnSettlingCount;
}

[GearTicks]

Thanks so much for the help! We now have our testbot able to accurately turn to 90 degrees or whatever angle we set it at. The problem we are now seeing is when we call the method for the GyroTurn into our while opmodeIsActive () it only allows us to turn. If we try to add any other commands such as driving forward it doesn't recognize what we are telling it to do. I am copying in what we have written showing what works.

@Autonomous(name="Gyro Turn", group ="McElroy")
//@Disabled
public class RelicRecoveryAutonomous extends LinearOpMode {

// Declare OpMode members.
private ElapsedTime runtime = new ElapsedTime();

// Declare DC Motors.
private DcMotor front_left;
private DcMotor back_left;
private DcMotor front_right;
private DcMotor back_right;

//Declare Servos

//Declare Sensors
BNO055IMU imu;

//User Generated Values
double headingResetValue;

@Override
public void runOpMode() {

telemetry.addData("Status", "Initialized");
telemetry.update();

//DC MOTORS
front_left = hardwareMap.get(DcMotor.class, "LF");
back_left = hardwareMap.get(DcMotor.class, "LB");
front_right = hardwareMap.get(DcMotor.class, "RF");
back_right = hardwareMap.get(DcMotor.class, "RB");

//SET THE DRIVE MOTOR DIRECTIONS
front_left.setDirection(DcMotor.Direction.FORWARD);
back_left.setDirection(DcMotor.Direction.FORWARD);
front_right.setDirection(DcMotor.Direction.REVERSE);
back_right.setDirection(DcMotor.Direction.REVERSE);

//SET THE RUN MODE FOR THE MOTORS
//back_left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//front_left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//back_right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//front_right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

//SERVOS

//SENSORS
this.imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
this.imu.initialize(parameters);

//parameters.calibrationDataFile = "BNO055IMUCalibration.json";
//parameters.loggingEnabled = true;
//parameters.loggingTag = "IMU";
//parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

//INITIALIZATIONS
this.headingResetValue = this.getAbsoluteHeading();

waitForStart();
runtime.reset();

while (opModeIsActive()) {

final double heading = this.getRelativeHeading();

boolean doneTurn1 = this.gyroCorrect(90.0, 1., heading, 0.1, 0.3) > 10;
this.telemetry.addData("Heading", heading);
this.telemetry.addData("Done Turn", doneTurn1);
}
}


//FUNCTIONS

//GYRO TURNING
private double getAbsoluteHeading(){
return this.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;
}

private double getRelativeHeading(){
return this.getAbsoluteHeading() - this.headingResetValue;
}

private int correctCount = 0;
/*
* @param gyroTarget The target heading in degrees, between 0 and 360
* @param gyroRange The acceptable range off target in degrees, usually 1 or 2
* @param gyroActual The current heading in degrees, between 0 and 360
* @param minSpeed The minimum power to apply in order to turn (e.g. 0.05 when moving or 0.15 when stopped)
* @param addSpeed The maximum additional speed to apply in order to turn (proportional component), e.g. 0.3
* @return The number of times in a row the heading has been in the range
*/
public int gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed, double addSpeed) {
double delta = (gyroTarget - gyroActual + 360.0) % 360.0; //the difference between target and actual mod 360
if (delta > 180.0) delta -= 360.0; //makes delta between -180 and 180
if (Math.abs(delta) > gyroRange) { //checks if delta is out of range
this.correctCount = 0;
double gyroMod = delta / 45.0; //scale from -1 to 1 if delta is less than 45 degrees
if (Math.abs(gyroMod) > 1.0) gyroMod = Math.signum(gyroMod); //set gyromod to 1 or -1 if the error is more than 45 degrees
this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
}
else {
this.correctCount++;
this.turn(0.0);
}
return this.correctCount;
}

public void turn(double sPower) {
front_left.setPower(- sPower);
back_left.setPower(- sPower);
front_right.setPower(+ sPower);
back_right.setPower(+ sPower);
}
}

Our team also uses a state-machine approach to allow us to compose reusable stages into complex autonomous routines, but that is probably overkill for your application. You can use the linear nature of the LinearOpMode to run actions in sequence. For example, to drive forward and then turn, you could have one loop running the drive code and then another after it to run the turn code, e.g.

//Drive forward
front_left.setMode(RunMode.STOP_AND_RESET_ENCODER);
front_left.setPower(1.0);
back_left.setPower(1.0);
front_right.setPower(1.0);
front_left.setPower(1.0);
while (this.opModeIsActive() && Math.abs(front_left.getCurrentPosition()) < 2000);

//Stop
front_left.setPower(0.0);
back_left.setPower(0.0);
front_right.setPower(0.0);
front_left.setPower(0.0);
this.sleep(200); //wait for robot to come to rest

//Turn
while (this.opModeIsActive() && this.gyroCorrect(90.0, 1., heading, 0.1, 0.3) < 10);

[KernelPanic]

.... If we try to add any other commands such as driving forward it doesn't recognize what we are telling it to do. I am copying in what we have written showing what works.

...
while (opModeIsActive()) {

    final double heading = this.getRelativeHeading();

    boolean doneTurn1 = this.gyroCorrect(90.0, 1., heading, 0.1, 0.3) > 10;
    this.telemetry.addData("Heading", heading);
    this.telemetry.addData("Done Turn", doneTurn1);
}
...

I haven't run this nor looked at it really hard. But whenever something gets stuck, loops & return values are really good places to start looking - like pulse and blood pressure during a physical exam.

In this case, the stuff in "while (opModeIsActive())" will get evaluated over and over. If your commands to drive straight are outside of that loop, they aren't going to be evaluated until after the show's over and the staff is vacuuming the floor and putting the chairs on the tables. That's a really good place to start looking, I think.

The problem is, that you probably want it to do its turn then do some other maneuver, and the turn functionality is totally dependent on getting hit in the loop. Adding multiple loops makes things more complicated (like by a factor of 2 for each loop, IIRC), and that's the origin of the earlier suggestion of a state machine.

A state machine sounds really complicated, but like a lot of things in life, it mimics what we do.
Think of the states involved with making a slice of toast.
1) put the toast in the toaster,
2) wait for the toaster to pop
3) remove the toast
4) apply your favorite spread
5) eat.

Typically, you stand there, subconsciously going "the toast is in the toaster. I cannot put butter on it yet" or "the toaster has popped, I must remove the toast"
In your robot's case, it would be something more like

...
int opState = 0;
while (opModeIsActive()){
    if (opState == 0) {
        // do the turn stuff.
        // if the turn reaches the target, increment the opState
        ...
        if (doneTurn1) opState++;
    }
    else if (opState == 1) {
        // do stuff to drive forward
        // when done driving forward, increment the opState
    }
    else if (opState == 2) {
        // I dunno, make some toast?  but you get the idea
    }
}

There are prettier ways of doing this, and a switch statement works really well.
Here's some prototype code I'm running on my desktop that uses a switch.
Only here, I've made CONSTANTS to say what kind of thing I want it to do, and put them in an array.
The index of the array is the thing that I increment, that way I can have far fewer states than actions.
For example, putting the toast into the toaster is pretty much the same as getting the toast out of the toaster, just that the directions are reversed. Do we really need two distinct states for that?
The following code was copied from an IDE and shows how a state machine like that might be implemented.

// while opmode is active() surrounds all of this.
       // states for the NAV switch statement
        final int CLM = 0; // clamp: uses clamping, power, timeLim
        final int MOV = 2; // move: compass, forward, crab, power, timeLim
        final int LFT = 3; // lifter: uses lifter, power, timeLim
        final int WT = 4;  // wait: uses timeLim

        int[] action =   {CLM,  LFT, MOV,  MOV,  CLM,  MOV,  WT};
...
     switch ( action[autoState] )
     {
            case Structures.CLM:  // operate the clamp
                myClamps =  workClamp(aSet.clamp[autoState],stageTimer, aSet.dur[autoState]);
                riserCmd = myClamps.liftMotor;
                stageComplete |= myClamps.status;
                break;
           case Structures.LFT:
                ...
           break;
               ...
           case Structures.WT:
                 break;
           default:
                 break;
        }
        if (stageComplete)
        {
              riserCmd = 0;
              mVector.startH = startHeading;
              stageTimer= 0;
              autoState++;
        }

Anyway, this is a copy of some prototype code I'm running in Eclipse (like Android Studio, but not Android Studio), and it runs but is buggy. It's kind of a playground for where I want our students to drive the code. It's in github at https://github.com/50N40W/java4ftc/tree/master/ftc and you're welcome to look at, play with, or fork it, just know that it is buggy, subject to change, and for experimental purposes.

[CoachZM]

Thanks so much for the help! We now have our testbot able to accurately turn to 90 degrees or whatever angle we set it at. The problem we are now seeing is when we call the method for the GyroTurn into our while opmodeIsActive () it only allows us to turn. If we try to add any other commands such as driving forward it doesn't recognize what we are telling it to do. I am copying in what we have written showing what works.

@Autonomous(name="Gyro Turn", group ="McElroy")
//@Disabled
public class RelicRecoveryAutonomous extends LinearOpMode {

    // Declare OpMode members.
    private ElapsedTime runtime = new ElapsedTime();

    // Declare DC Motors.
    private DcMotor front_left;
    private DcMotor back_left;
    private DcMotor front_right;
    private DcMotor back_right;

    //Declare Servos

    //Declare Sensors
    BNO055IMU imu;

    //User Generated Values
    double headingResetValue;

    @Override
    public void runOpMode() {

        telemetry.addData("Status", "Initialized");
        telemetry.update();

        //DC MOTORS
        front_left = hardwareMap.get(DcMotor.class, "LF");
        back_left = hardwareMap.get(DcMotor.class, "LB");
        front_right = hardwareMap.get(DcMotor.class, "RF");
        back_right = hardwareMap.get(DcMotor.class, "RB");

        //SET THE DRIVE MOTOR DIRECTIONS
        front_left.setDirection(DcMotor.Direction.FORWARD);
        back_left.setDirection(DcMotor.Direction.FORWARD);
        front_right.setDirection(DcMotor.Direction.REVERSE);
        back_right.setDirection(DcMotor.Direction.REVERSE);

        //SET THE RUN MODE FOR THE MOTORS
        //back_left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        //front_left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        //back_right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        //front_right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        //SERVOS

        //SENSORS
        this.imu = hardwareMap.get(BNO055IMU.class, "imu");
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        this.imu.initialize(parameters);

        //parameters.calibrationDataFile = "BNO055IMUCalibration.json";
        //parameters.loggingEnabled = true;
        //parameters.loggingTag = "IMU";
        //parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();

        //INITIALIZATIONS
        this.headingResetValue = this.getAbsoluteHeading();

        waitForStart();
        runtime.reset();

        while (opModeIsActive()) {

            final double heading = this.getRelativeHeading();

            boolean doneTurn1 = this.gyroCorrect(90.0, 1., heading, 0.1, 0.3) > 10;
            this.telemetry.addData("Heading", heading);
            this.telemetry.addData("Done Turn", doneTurn1);
        }
    }


    //FUNCTIONS

    //GYRO TURNING
    private double getAbsoluteHeading(){
        return this.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;
    }

    private double getRelativeHeading(){
        return this.getAbsoluteHeading() - this.headingResetValue;
    }

    private int correctCount = 0;
    /*
     * @param gyroTarget The target heading in degrees, between 0 and 360
     * @param gyroRange The acceptable range off target in degrees, usually 1 or 2
     * @param gyroActual The current heading in degrees, between 0 and 360
     * @param minSpeed The minimum power to apply in order to turn (e.g. 0.05 when moving or 0.15 when stopped)
     * @param addSpeed The maximum additional speed to apply in order to turn (proportional component), e.g. 0.3
     * @return The number of times in a row the heading has been in the range
*/
    public int gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed, double addSpeed) {
        double delta = (gyroTarget - gyroActual + 360.0) % 360.0; //the difference between target and actual mod 360
        if (delta > 180.0) delta -= 360.0; //makes delta between -180 and 180
        if (Math.abs(delta) > gyroRange) { //checks if delta is out of range
            this.correctCount = 0;
            double gyroMod = delta / 45.0; //scale from -1 to 1 if delta is less than 45 degrees
            if (Math.abs(gyroMod) > 1.0) gyroMod = Math.signum(gyroMod); //set gyromod to 1 or -1 if the error is more than 45 degrees
            this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
        }
        else {
            this.correctCount++;
            this.turn(0.0);
        }
        return this.correctCount;
    }

    public void turn(double sPower) {
        front_left.setPower(- sPower);
        back_left.setPower(- sPower);
        front_right.setPower(+ sPower);
        back_right.setPower(+ sPower);
    }
}

[GearTicks]

GearTicks Where do you put : "imu.getAngularOrientation(AxesReference.INTRINSIC , AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle" in your code and also where does the getRelativeHeading() become defined so it is recognized in the Gyro Turn function?

It all depends on the structure of your code. If you are only using it in one OpMode, you can do something like this (of course, having implemented the turn() method):

public class GyroTurnTest extends OpMode {
    private BNO055IMU imu;
    private double headingResetValue;

    public void init() {
        this.imu = (BNO055IMU)this.hardwareMap.get("bno"); //or whatever it is named in your configuration file
        final BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
        parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
        this.imu.initialize(parameters);
    }
    public void start() {
        this.headingResetValue = this.getAbsoluteHeading();
    }
    public void loop() {
        final double heading = this.getRelativeHeading();
        final boolean doneTurn = this.gyroCorrect(90.0, 1.0, heading, 0.1, 0.3) > 10;
        this.telemetry.addData("Heading", heading);
        this.telemetry.addData("Done turn", doneTurn);
    }

    private double getAbsoluteHeading() {
        return this.imu.getAngularOrientation(AxesReference.INTRINSIC , AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;
    }
    private double getRelativeHeading() {
        return this.getAbsoluteHeading() - this.headingResetValue;
    }

//GYRO-CONTROLLED TURN CODE
    private int correctCount = 0;
    /**
     * @param gyroTarget The target heading in degrees, between 0 and 360
     * @param gyroRange The acceptable range off target in degrees, usually 1 or 2
     * @param gyroActual The current heading in degrees, between 0 and 360
     * @param minSpeed The minimum power to apply in order to turn (e.g. 0.05 when moving or 0.15 when stopped)
     * @param addSpeed The maximum additional speed to apply in order to turn (proportional component), e.g. 0.3
     * @return The number of times in a row the heading has been in the range
     */
    public int gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed, double addSpeed) {
        double delta = (gyroTarget - gyroActual + 360.0) % 360.0; //the difference between target and actual mod 360
        if (delta > 180.0) delta -= 360.0; //makes delta between -180 and 180
        if (Math.abs(delta) > gyroRange) { //checks if delta is out of range
            this.correctCount = 0;
            double gyroMod = delta / 45.0; //scale from -1 to 1 if delta is less than 45 degrees
            if (Math.abs(gyroMod) > 1.0) gyroMod = Math.signum(gyroMod); //set gyromod to 1 or -1 if the error is more than 45 degrees
            this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
        }
        else {
            this.correctCount++;
            this.turn(0.0);
        }
        return this.correctCount;
    }
    public void turn(double sPower) {
        //put code here to turn the robot at the given power
    }
}

If you want to use it across multiple OpModes, you should create another class to keep track of the robot hardware (IMU, drive train, gyro correction, etc.) that will handle the common functionality.

[CoachZM]

GearTicks Where do you put : "imu.getAngularOrientation(AxesReference.INTRINSIC , AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle" in your code and also where does the getRelativeHeading() become defined so it is recognized in the Gyro Turn function?

[GearTicks]

GearTicks In your sample code you have double gyroActual as if we need to put a value into this. Wouldn't this be a value that has to be read by seeing what current position in the rotation the robot is in? Could you try to clarify this for us or even provide an example of that code being used in an example where we would want to turn from 0 degrees to 90 degrees? Thanks!

Yes, the function I provided calculates a turn power based on the current heading information, so it will work regardless of how you obtain your heading. Because the function does not calculate the heading for you, you will need to calculate it elsewhere and pass it into the function. We use a BNO055, so to get the absolute heading we call imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle. We reset the heading when we are in a known orientation and measure all future headings relative to the heading at the time of the reset. So to turn to 90 degrees, we would repeatedly call something like gyroCorrect(90.0, 1.0, getRelativeHeading(), 0.15, 0.3).

[CoachZM]

GearTicks In your sample code you have double gyroActual as if we need to put a value into this. Wouldn't this be a value that has to be read by seeing what current position in the rotation the robot is in? Could you try to clarify this for us or even provide an example of that code being used in an example where we would want to turn from 0 degrees to 90 degrees? Thanks!

[KernelPanic]

"Time," Douglas Adams wrote, "is an illusion." There is some mass & inertia with the robot, and using some sort of closed loop heading algorithm maybe will give fits sometimes as commands and feedback are not coming at predictable intervals.

For instance, if you're getting gyro headings inside a really tight loop, some of your data is probably stale (outdated), and in fact, there may be some limitations on the comm pipe between the sensor and the RC, (and for driver mode, between the RC and DS.) I thought I read the sensor value itself is only going to be updated every 30 milliseconds. If you're pinging it slower than that, you should have fairly predictable data, although it might help to think of it as "mine-crafty" - not super pretty.

We also feed it a "turnTime" indicating how long this has been underway and a BUMP_TIME indicating how long until we goose the power, plus a MAX_TURN_TIME, at which point we give up because something is in the way.
The first iteration of this was like 100 lines, but like making tomato paste, we kept boiling it down until we got it to four-ish. I think you could do it in one, but it wouldn't be easy to read. We invoke this every 50MS from the main navigation section, but only when a turn is commanded. I've edited it a little for brevity and clarity.

I don't think it does anything the above code doesn't, and may in fact have some limitations that doesn't even though they're both pretty similar. However, it's really short, and I've provided a lot of comments.

public float gyroTurn5(int currH, int newH, float pwrSet, float turnTime)
{

float returnPwr;
int cw = (pwrSet < 0)? -1: 1; // [1]
int transit = (((currH > newH) && (cw > 0)) ||

((currH < newH) && (cw < 0))) ? 360 : 0; // [2]

int desiredRotation = Math.abs(transit + (cw*newH) + ((-1*cw)*currH)); //[3]
desiredRotation = (desiredRotation > 360) ? desiredRotation - 360 : desiredRotation; //[4]
returnPwr = (desiredRotation > 0)? pwrSet : 0;
return returnPwr;

}

[1] cw is "clockwise" if the power command is positive, we are to turn clockwise.

Otherwise, anti-clockwise This makes a 180 degree difference when calculating how much turn is left
for this kind of turn, the returnPwr is applied to the right wheels,

(-1) * returnPwr goes to the left, at the place where this is called.
if the returnPwr is zero, the autonomous state machine advances to the next step.
... maybe think of it like a combo lock with a tumbler latching into place.
either way, the power setting happens to all motors in a different method.
That way we can create combinations of things and only set the power ONE place in the code.

[2] transit if the current heading is > target AND we're clockwise, then we transit the pole (go through 0 deg)

or if the current heading is less than the target AND we're counter clockwise, same thing

[3] desiredRotation the desired number of degrees of rotation depends on target (newH), currH,

AND whether clockwise or anti-clockwise

[4] desiredRotation if our desired amount of turn > 360, subtract 360.

the rest of it is just setting power requests depending on whether we've integrated enough turn degrees or timed out or whatever.
like when desiredRotation is still > 0 and we haven't timed out, set the power to whatever was requested.
That part was edited out for clarity, but the fun bit is posted here.
It's pretty straightforward, once you know this gets hit every 50MS, to calculate a rotation rate and bump power up
or down depending on how many degrees/second you want to see. If you don't know how often you're hitting the calculations,
determining degrees/sec can be a little dicey.

[CoachZM]

GearTicks thanks for the advice and sample. I am having some problems with helping my programmer understand the code you provided. I sent you an email with some more specifics. If you could help us learn a little more about it that would be greatly appreciated. Thanks!