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!