Hallo,

I'm having trouble writing a function:

```
float turnToRequestedHeading(float initialHeading, float requiredHeading, float turnRate)

```

I keep thinking there must be a clever way to do it, but it escapes me.

All values are in Radians, Headings between -PI and +PI, and turnRate between -0.5 and +0.5.

If the requiredHeading is less than the turnRate away from the initialHeading then it should return requiredHeading

Otherwise it should return initialHeading + or - turnRate, whichever gets closer to the requiredHeading.

Any ideas? I get stuck when the headings are either side of straight down, e.g. -3 and +3.

UPDATE: Here is some test code and test data (see my answer for my code below):

```
private void turnToRequestedHeadingTest(float initialHeading, float requiredHeading, float turnRate, float expectedResult) {
    if (Math.round(turnToRequestedHeading(initialHeading*PIf/180, requiredHeading*PIf/180, turnRate*PIf/180)*180/PIf) != expectedResult) {
        /*DEBUG*/Log.i(this.getClass().getName(), "test(initial="+initialHeading+", required="+requiredHeading+", rate="+turnRate+") Expected "+expectedResult+", Returns "+(Math.round(turnToRequestedHeading(initialHeading*PIf/180, requiredHeading*PIf/180, turnRate*PIf/180)*180/PIf)));
    }
}

    /*DEBUG*/Log.i(this.getClass().getName(), "turnToRequestedHeading tests:");
    turnToRequestedHeadingTest(   0,   0,  0,   0);
    turnToRequestedHeadingTest(   0,   0, 25,   0);
    turnToRequestedHeadingTest(  10,  15, 25,  15);
    turnToRequestedHeadingTest(  20,  55, 25,  45);
    turnToRequestedHeadingTest(  85,  95, 25,  95);
    turnToRequestedHeadingTest( 150,-170, 25, 175);
    turnToRequestedHeadingTest( 170, 177, 25, 177);
    turnToRequestedHeadingTest( 170,-175, 25,-175);
    turnToRequestedHeadingTest( 175,-100, 25,-160);
    turnToRequestedHeadingTest( 175,   0, 25, 150);
    turnToRequestedHeadingTest( 180,   0, 25, 155);
    turnToRequestedHeadingTest(-170,-100, 25,-145);
    turnToRequestedHeadingTest(-100, -80, 25, -80);
    turnToRequestedHeadingTest( -30, -15, 25, -15);
    turnToRequestedHeadingTest( -30,  15, 25,  -5);
    turnToRequestedHeadingTest( -20,  -5, 25,  -5);
    turnToRequestedHeadingTest( -20,   5, 25,   5);
    turnToRequestedHeadingTest( -20,  15, 25,   5);
    turnToRequestedHeadingTest(  10, 180, 25,  35);
    turnToRequestedHeadingTest(  10,-160, 25, -15);
    turnToRequestedHeadingTest( 170,   0, 25, 145);
    turnToRequestedHeadingTest( 170, -15, 25,-165);
    turnToRequestedHeadingTest(-170,   5, 25,-145);
    turnToRequestedHeadingTest( -10, 160, 25,  15);
    turnToRequestedHeadingTest( -10,-150, 25, -35);
    turnToRequestedHeadingTest(  10,-170, 25, -15);
    turnToRequestedHeadingTest(   0, 180, 25,  25);
    turnToRequestedHeadingTest( -10, -15, 25, -15);
    turnToRequestedHeadingTest( -20, -55, 25, -45);
    turnToRequestedHeadingTest( -85, -95, 25, -95);
    turnToRequestedHeadingTest(-150, 170, 25,-175);
    turnToRequestedHeadingTest(-170,-177, 25,-177);
    turnToRequestedHeadingTest(-170, 175, 25, 175);
    turnToRequestedHeadingTest(-175, 100, 25, 160);
    turnToRequestedHeadingTest(-175,   0, 25,-150);
    turnToRequestedHeadingTest( 170, 100, 25, 145);
    turnToRequestedHeadingTest( 100,  80, 25,  80);
    turnToRequestedHeadingTest(  30,  15, 25,  15);
    turnToRequestedHeadingTest(  30, -15, 25,   5);
    turnToRequestedHeadingTest(  20,   5, 25,   5);
    turnToRequestedHeadingTest(  20,  -5, 25,  -5);
    turnToRequestedHeadingTest(  20, -15, 25,  -5);
    turnToRequestedHeadingTest( -10,-180, 25, -35);
    turnToRequestedHeadingTest( -10, 160, 25,  15);
    turnToRequestedHeadingTest(-170,   0, 25,-145);
    turnToRequestedHeadingTest(-170,  15, 25, 165);
    turnToRequestedHeadingTest( 170,  -5, 25, 145);
    turnToRequestedHeadingTest(  10,-160, 25, -15);
    turnToRequestedHeadingTest(  10, 150, 25,  35);
    turnToRequestedHeadingTest( -10, 170, 25,  15);
    // More tests
    turnToRequestedHeadingTest(   0,  15, 25,  15);
    turnToRequestedHeadingTest(   0,  60, 25,  25);
    turnToRequestedHeadingTest(   0, -15, 25, -15);
    turnToRequestedHeadingTest(   0, -60, 25, -25);
    turnToRequestedHeadingTest( 180, 165, 25, 165);
    turnToRequestedHeadingTest( 180, 100, 25, 155);
    turnToRequestedHeadingTest( 180,-165, 25,-165);
    turnToRequestedHeadingTest( 180,-100, 25,-155);
    turnToRequestedHeadingTest(-180, 165, 25, 165);
    turnToRequestedHeadingTest(-180, 100, 25, 155);
    turnToRequestedHeadingTest(-180,-165, 25,-165);
    turnToRequestedHeadingTest(-180,-100, 25,-155);
    turnToRequestedHeadingTest(  25,   0, 25,   0);
    turnToRequestedHeadingTest(  25, -25, 25,   0);
    turnToRequestedHeadingTest( -25,   0, 25,   0);
    turnToRequestedHeadingTest( -25,  25, 25,   0);
    turnToRequestedHeadingTest( 155, 180, 25, 180);
    turnToRequestedHeadingTest( 155,-155, 25, 180);
    turnToRequestedHeadingTest(-155, 180, 25,-180);
    turnToRequestedHeadingTest(-155, 155, 25,-180);
    turnToRequestedHeadingTest( 155,-180, 25,-180);
    turnToRequestedHeadingTest(-155,-180, 25,-180);

```

I think my test data covers all cases now...

-Frink