NavX MXP Continuous Angle to Calculate Derivative

[lethc]

For our autoalignment sequence, we have an algorithm to 'find' the correct motor speed necessary to turn the robot at a certain velocity. The algorithm bumps up motor speed every 100ms until the robot is turning at a certain very slow velocity. The NavX's getRate() function had very low resolution so we wrote a class to calculate the average change in yaw over time (derivative). However, for this to work at all headings we'd need a method on the NavX to return a continuous angle. Currently both getAngle() and getYaw() are not continuous, and their ranges are [0, 360] and [-180, 180] respectively, so at the point where the robot crosses the threshold of a range the values jump ~360 degrees which makes the robot think that it just turned the full 360 degrees in a matter of milliseconds.

Does anyone have any ideas on how I would get a continuous angle heading from the NavX? I.E. one that doesn't jump from 360 to 0 instantly, because that messes up the derivative calculation.

To get around this issue currently, we're checking if the derivative is less than something like 30 deg/s, and if it isn't just return 0 deg/s, but that's really hacky and I'd like to actually fix the problem.

My team is using Java. Thanks for your help!

[virtuald]

I think that was a software bug... have you updated to the latest software?

[dougwilliams]

Originally Posted by lethc

For our autoalignment sequence, we have an algorithm to 'find' the correct motor speed necessary to turn the robot at a certain velocity....
!

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360. I'm not sure how you have it configured but on our we have the issue because it jumps from -180 / 180.

Question though - what are you trying to achieve by closing the loop to a specific turning speed? We use the same setup and close the loop on the current field orientation angle. In that way you can have the robot quickly turn when it's farther away from the set point, and slow down as it approaches where you want it to be so you have minimal overshoot.

[JamesTerm]

Would x=x%180.... (modulo operator) work? May need to preserve sign.

[notmattlythgoe]

Originally Posted by dougwilliams

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360. I'm not sure how you have it configured but on our we have the issue because it jumps from -180 / 180.

Question though - what are you trying to achieve by closing the loop to a specific turning speed? We use the same setup and close the loop on the current field orientation angle. In that way you can have the robot quickly turn when it's farther away from the set point, and slow down as it approaches where you want it to be so you have minimal overshoot.

One thing we've been trying is using a closed loop to control the rate at which the robot turns and then use a P controller to tell the first controller how fast to turn based on how far away from our target angle we are.

[slibert]

Originally Posted by lethc

For our autoalignment sequence, we have an algorithm to 'find' the correct motor speed necessary to turn the robot at a certain velocity. The algorithm bumps up motor speed every 100ms until the robot is turning at a certain very slow velocity. The NavX's getRate() function had very low resolution so we wrote a class to calculate the average change in yaw over time (derivative). However, for this to work at all headings we'd need a method on the NavX to return a continuous angle. Currently both getAngle() and getYaw() are not continuous, and their ranges are [0, 360] and [-180, 180] respectively, so at the point where the robot crosses the threshold of a range the values jump ~360 degrees which makes the robot think that it just turned the full 360 degrees in a matter of milliseconds.

Does anyone have any ideas on how I would get a continuous angle heading from the NavX? I.E. one that doesn't jump from 360 to 0 instantly, because that messes up the derivative calculation.

To get around this issue currently, we're checking if the derivative is less than something like 30 deg/s, and if it isn't just return 0 deg/s, but that's really hacky and I'd like to actually fix the problem.

My team is using Java. Thanks for your help!

Can you please ensure that you are using the latest NavX-MXP libraries? There was a change checked in on Feb 20 of this year that fixed a bug in the getAngle() code. This should be returning a value that is not constrained to 0-360 degrees. Since you've indicated you are seeing a range of 0-360, I'm suspicious somethings out of date.

Also note there's a recent firmware release which increases the update rate maximum to 100Hz, so it might be worth downloading the latest release, running the setup to update the libraries, and updating the firmware.

Please feel free to contact support@kauailabs.com if you still have trouble.

[Ether]

Originally Posted by dougwilliams

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360.

Originally Posted by JamesTerm

Would x=x%180.... (modulo operator) work?

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

EDIT:

Test code:

#include <math.h>
#include <stdio.h>

double shortest_angle(double previous, double present){
	return (present-previous) - 360.0*floor(0.5+(present-previous)/360.0);}

void test(double previous, double present){
	printf("previous=%f\tpresent=%f\tshortest_angle=%f\n",previous,present,shortest_angle(previous,present));
	}

void main(void){
	test(360,0);
	test(0,360);
	test(360,1);
	test(1,360);
	test(359,0);
	test(0,359);
	test(1,359);
	test(359,1);
	test(359,-1);
	test(-1,359);
	test(3*360+1,-1);
	test(-1,3*360+1);
	}

Output:

previous=360.000000     present=0.000000        shortest_angle=0.000000
previous=0.000000       present=360.000000      shortest_angle=0.000000
previous=360.000000     present=1.000000        shortest_angle=1.000000
previous=1.000000       present=360.000000      shortest_angle=-1.000000
previous=359.000000     present=0.000000        shortest_angle=1.000000
previous=0.000000       present=359.000000      shortest_angle=-1.000000
previous=1.000000       present=359.000000      shortest_angle=-2.000000
previous=359.000000     present=1.000000        shortest_angle=2.000000
previous=359.000000     present=-1.000000       shortest_angle=0.000000
previous=-1.000000      present=359.000000      shortest_angle=0.000000
previous=1081.000000    present=-1.000000       shortest_angle=-2.000000
previous=-1.000000      present=1081.000000     shortest_angle=2.000000

¹ REMAINDER function "x REM y" per IEC 60559 as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3

[lethc]

Originally Posted by dougwilliams

Question though - what are you trying to achieve by closing the loop to a specific turning speed?

We're not running it closed loop. We've implemented a system described in the paper here. Essentially assuming all variables stay the same, powering motors at the same speed for a certain number of 'pulses'/time should send it to very similar locations each time. We used this at the Oklahoma regional but found that using one certain voltage to run the motors was too prone to the variables in our robot, like battery voltage, tire pressure and internal drivetrain friction. To fix this we implemented a system that starts powering the drivetrain at a small power and incrementally steps up the power until the robot begins to move at the speed we want it to. We then use this found speed to run our 'loops cycles'. By doing this our inconsistency problems have been mostly solved (at the Oklahoma regional we missed 3/4 of the last 4 auto shots because our battery voltages had become so consistently high).

Originally Posted by Ether

The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

Just tried this out and it works perfectly. Thank you for your help.

I haven't updated the NavX libraries yet, as we leave for Missouri State Champs at 3:00 tomorrow and we're in a time crunch. Ether's fix seemed the quickest and easiest.

Thank you everyone for all the help! I can post source code for our autoalignment command and the derivative calculator in a few days if anyone is interested.

[Jared Russell]

This year on 254 we switched to using a simple Rotation2d class for all angles (because, among other reasons, dealing with angle rollover as you did here is easy to screw up). Internally, this class stores the sine and cosine of an angle explicitly. This has some nice properties:

• We use static utility methods to create a Rotation2d from a vector (x,y -> cos,sin after normalization), absolute angle in radians, or absolute angle in degrees.
• We have accessors that return the absolute angle in degrees or radians (wrapped to -Pi to Pi).
• We have "Inverse()" and "RotateBy(Rotation2d other)" methods for creating a new Rotation2d:
  

  Rotation2d Inverse() {
    return new Rotation2d(cos_angle, -sin_angle);
  }
  
  Rotation2d RotateBy(Rotation2d other) {
    return new Rotation2d(cos_angle * other.cos_angle - sin_angle * other.sin_angle,
                  cos_angle * other.sin_angle + sin_angle * other.cos_angle);
  }

• If you want to know the shortest rotation from A to B, you can do something like:
  

  double shortest_distance_degrees = A.Inverse().RotateBy(B).ToDegrees();

  This will always give you a solution on [-180, 180].
• Cheap access to the sine, cosine, and tangent (sin/cos) of the angle because they are already cached.

[JamesTerm]

Thanks Ether... learn something new everyday!

The way Microsoft's library present's this function is like so:
IEEERemainder = dividend - (divisor * Math.Round(dividend / divisor))

It's great to see such a compatible use-case for this function as written in this thread... this function has also helped improve some bugs in my simulations as well.

[JamesTerm]

Here is a c++ equivalent within standard libraries:
http://en.cppreference.com/w/cpp/numeric/math/remainder

[Richard100]

Originally Posted by Ether

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

Some practical considerations regarding this approach ...

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

Also be aware that the function is sensitive to sampling rate. This is because a late sample might allow a current position (the 'present' variable) to get more than 180 degrees away the previous position (the 'previous' variable). If this happens then the shortest angle will switch to the other side of the cycle. If you've ever noticed movies where wagon or car wheels appear to be moving backwards then you've seen this aliasing effect (this can happen to your sensor, too).

To avoid this, ensure the iteration period is at least shorter than (1 / 2*RPS), where RPS is the maximum revolutions per second expected from the sensor reading stream. For example, if your robot maximum yaw rotation rate is 1000 deg/sec, you must iterate the function no slower than every 180 ms ... (someone check my math).

This is likely not a problem for most FRC robots, as yaw rates don't normally get that high (ours don't get much higher than 500 deg/sec), and computational iteration rates are probably much faster - but one should be aware of the constraint. You might try the function on a robot mechanism that rotates much faster and wonder why your code is suddenly unreliable.

If the sensor readings are noisy, this will further erode the sampling margin. If the peak noise level in degrees is 'n', then the sampling constraint becomes:

Loop Period < (180 - 2*n)/(360 * RPS)

Of course, excessive noise should be remedied (fixing the root cause). One should just keep in mind that it's good practice to maintain healthy margin for practical issues like noise, and in fact whether your loop can always be trusted to occur on schedule.

The exploit of the shortest directional path property of the IEEERemainder function is quite clever for dealing with these orientation discontinuities. I wasn't aware of it until Ether pointed it out - (thank you Ether). It doesn't appear that LabVIEW has this function (although easy enough to build from primitives) ... unless I'm just not finding it - anyone know for sure?

[Ether]

Originally Posted by Richard100

The exploit of the shortest directional path property of the IEEERemainder function is quite clever for dealing with these orientation discontinuities. I wasn't aware of it until Ether pointed it out - (thank you Ether). It doesn't appear that LabVIEW has this function (although easy enough to build from primitives) ... unless I'm just not finding it - anyone know for sure?

Look at the Endnote at the bottom of the last page.

[Ether]

Originally Posted by Richard100

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

It's not clear what you're saying here.

You don't need an accumulator when using a gyro if all you care about is your heading.

The desired heading and the gyro angle do not have a range constraint in this case.

For example, the gyro angle could be 721 degrees and the desired heading could be -1 degree. The function would return -2 degrees as the shortest angle, which is the value you want.

[MichaelBick]

Originally Posted by notmattlythgoe

One thing we've been trying is using a closed loop to control the rate at which the robot turns and then use a P controller to tell the first controller how fast to turn based on how far away from our target angle we are.

We did something similar this year. We ended up running a rotational velocity P controller with feedforward as the baseline of our turn controller. We summed the output without the feedforward, and limited to +-20% voltage to help prevent windup. A PD controller on turning angle was the input to the velocity controller.

There was definitely a noticeable improvement on the conventional PID controller. One problem we were facing without the velocity controller was that our turns were highly dependent on momentum. We really would have liked to run the velocity loop underneath our teleop driving code too, but integral windup caused a lot of unpredictable behavior over long periods of time.