Turn Wrapping

Why Turn Wrapping is Needed

For the robot to make a turn or stay at a specific heading, the PID program will have to know the angle to turn to. This sounds easy, but a few things make this more complicated then it seems. The gyro counts up or down in degrees, meaning it can range anywhere from -infinity to infinity. This is also how the PID controller will require the target angle, as PID works on a number line, simply trying to move a variable up or down. The angle we want to supply, however, is from 0-360. This presents a problem: if the PID system is told to turn to 0 degrees while the gyro reads, for example, 380, the robot will turn more than one full rotation to reach a target just 20 degrees away. What we need is a function that finds the closest conterminal angle. Coterminal angles of 0 would be 360, 720, and onward to infinity by multiples of 360. Here, 0 would be more than a rotation away, 720 slightly less than a rotation, but 360 only 20 degrees away, thus it would be the closest coterminal angle. We need a function that finds this, so we can deal with what angle we need to be at from 0-360, while still making PID easy to program and the gyro simple to use.

The Math of Turn Wrapping

Modular Arithmetic

Before jumping into the exact math, it's important to introduce the concept of modular arithmetic. You're probably already familiar with how modular arithmetic works, but you might not have seen the math or coding syntax for it. Modular arithmetic is the idea behind things like a clock: in a 12 hour day, hours count up, then go back to 0 at 12. If something took place 15 hours after noon, it would take place at 3, not 15. A mathematical way of expressing this is 15 modulo 12 = 3. Modulo is often shorted to mod, and sometimes use % as an operator like + or *. This is incredibly useful when dealing with angles, since it allows us to find where a number is on the 0-360 scale if it's from -infinity to infinity, for instance 720 % 360 = 0, or 740 % 360 = 20. Mod also works within 0-360, for instance 40 % 360 = 360. Certain kinds of Mod functions in certain programming languages and libraries also work for negatives: -5 % 360 = 355. Here's a good explanation of why some functions don't work well for negatives, and why for this program we'll be using the floorMod function.

Creating the Turn Wrapping function/algorithm

Before we can code the Turn Wrapping function, we need to figure out the math we'll actually be using. Our PID program will need the angle to turn to in the -infinity to infinity range, and the function we'll be writing will take in the target from 0-360. So, we'll need to find the closest coterminal angle of our target to our current angle. To do this, we'll find the nearest coterminal angle above our current angle, then below our current angle, although the function will actually find the difference, or delta, from the current angle to those coterminal angles because of the way the math we're using works. TODO: MATH EXPLANATION You can see this math animated here.

Coding Turn Wrapping

Now that we have our two math functions, let's implement the closestAngle method that's in mathUtils. We'll get rid of it's current placeholder method, and set a firstTargetDelta and secondTargetDelta method that is set to the first and second math equation we use:

    public static double closestAngle(double targetAngle, double currentAngle) {
        double simpleTargetDelta = floorMod(((360 - targetAngle) + currentAngle), 360);
        double alternateTargetDelta = -1 * (360 - simpleTargetDelta);
    }

You may, however, notice that this will give us some errors. The reason is java's floorMod method only will take integers for reason that I still don't understand despite teaching this class have to do with very complicated math concepts that would take a while to explain despite me completely understanding them. To fix this, we'll just have to make them integers by rounding off the decimal points, since we don't need any more than a degree of precision for turning. Thus, we'll use java's Math.round function to round off the decimal points:

    public static double closestAngle(double targetAngle, double currentAngle) {
        double simpleTargetDelta = floorMod(Math.round((360 - targetAngle) + currentAngle), 360);
        double alternateTargetDelta = -1 * (360 - simpleTargetDelta);
    }

Now, we need to figure out which has a smaller absolute value, then return what our new PID setPoint will be, by subtracting the result from our currentAngle. To do this, we're going to use a "condensed" version of an if statement, called a ternary operator. A ternary operator works like this:

variable = (condition) ? expressionTrue :  expressionFalse;

As an example (taken from W3Schools), here's what something would look like using an if statement:

int time = 20;
if (time < 18) {
  System.out.println("Good day.");
} else {
  System.out.println("Good evening.");
}

Whereas using the ternary operator would look like:

int time = 20;
String result = (time < 18) ? "Good day." : "Good evening.";
System.out.println(result);

So let's construct our return statement. Let's look at that ternary example again:

variable = (condition) ? expressionTrue :  expressionFalse;

In this case, our condition will look like this:

StrictMath.abs(simpleTargetDelta) <= StrictMath.abs(alternateTargetDelta)

Our expression if true this:

currentAngle - simpleTargetDelta

Our expression if false this:

currentAngle - alternateTargetDelta

And so our final method will look like this, by returning a ternary operator:

    //finds the closest coterminal angle of targetAngle to currentAngle
    public static double closestAngle(double targetAngle, double currentAngle) {
        double simpleTargetDelta = floorMod(Math.round((360 - targetAngle) + currentAngle), 360);
        double alternateTargetDelta = -1 * (360 - simpleTargetDelta);
        return StrictMath.abs(simpleTargetDelta) <= StrictMath.abs(alternateTargetDelta) ? currentAngle - simpleTargetDelta : currentAngle - alternateTargetDelta;
    }

Congratulations! We now have a closestAngle method. If we pass in 0 as our targetAngle and 700 as our currentAngle, it's result will be 720. It works! Now, let's get to implementing it. Right now, we'll add the functionality of turning to cardinal directions (0, 90, 180, and 270) by pressing buttons on the controller's D-pad.

Using Turn Wrapping

Let's go into our Tele-Op now, and add some functionality to make the robot turn to those directions. We'll be getting rid of the temporary code we wrote for PID that turns 90 degrees to the side of the robot, and make our robot actually turn to exactly 90 degrees! Once we've deleted the if statement that sets our setPoint to += 90, we can add another statement to set our setpoint. This will check if the correct D-pad button has been tapped, then sets setPoint to the result of a call to closestAngle using the correct angle and our IMU.getAngle() method that returns our current angle:

        //sets setPoint to be the closest coterminal angle of 0
        if(Controller1.up.isTapped()){
            setPoint = MathUtils.closestAngle(0, IMU.getAngle());
        }

Now, we'll fill in these statements for the rest of our controller:

        if(Controller1.up.isTapped()){
            setPoint = MathUtils.closestAngle(0, IMU.getAngle());
        }
        if(Controller1.right.isTapped()){
            setPoint = MathUtils.closestAngle(90, IMU.getAngle());
        }
        if(Controller1.down.isTapped()){
            setPoint = MathUtils.closestAngle(180, IMU.getAngle());
        }
        if(Controller1.left.isTapped()){
            setPoint = MathUtils.closestAngle(90, IMU.getAngle());
        }

Now, when we press D-pad buttons, our robot should automatically turn to the correct angle, without turning around multiple times.