navX MXP PID Drift Compensation

Hello! Our team is using a mecanum chassis this year, and we are attempting to use a navX MXP to compensate for drifting. Our current issue is getting the robot to set its header at the right time. Currently, Teleop stores a header whenever we are rotating the robot. However, we are having trouble with it over-rotating and then the PID loop snapping it back. Whenever we let go of our trigger, the robot tries to snap back to that exact spot. We thought to get past this issue by only activating the PID loop when the robot’s angular velocity is below a certain threshold, but we cannot find anywhere in the navX MXP’s libraries that will allow us to read the angular velocity. We would also like to change it from constantly changing the header during a turn and to instead set a header immediately upon stopping rotating (you’ll see our attempt at this with a feedback node in the attacked image, but it ends up never changing the header). Any help with either of these problems is much appreciated!
Any cases not shown are completely unimportant. I promise. :smiley:

The straightforward approach is to subtract the current yaw angle from the previous yaw angle. The difference is the instantaneous angular velocity (degrees/sec/update_rate_in_hertz).

If you prefer the angular velocity in degrees/sec, then multiply the instantaneous angular velocity by the update rate. Or if you really want the exact angular velocity over some period of longer time than the update rate period, the robot app could integrate some number of instantaneous angular velocity values over some time period.

We could add the calculation in the navXAE LabVIEW library, but if you need a solution soon I’d recommend the above.

Alright, thanks for your help! But yeah, having a block to pull the angular velocity from (especially in rad/sec) would be immensely useful going forward.

And then if anybody knows what we did wrong with our feedback loop, please let me know. Thanks.

We’re actually having some trouble getting this. You use a feedback loop to store the previous iteration’s data and then compare them, yes?

That’s the idea. The navX yaw value is placed into the iteration data, and the next time through the loop the new yaw value has the previous value (in the iteration data) subtracted from it.

So if the last yaw angle was: 0.5 degrees
and if the new yaw angle was: 0.6 degrees

Then the instantaneous angular velocity between the last and new values would be 0.6 - 0.5 = 0.1 degrees.

Taking that one step further, if your current update rate is 50 Hz, then you can estimate that within the last second, the robot likely rotated by 0.1 * 50 = 5 degrees/sec (assuming continuous motion).

Just realized I never followed up on this - we had it right, we had just miscalculated our update rate, so we were really off on our numbers.

Thanks for your help!

Did NavX ever add in Angular Velocity? Am I right in thinking its cont yaw?

Good question - would make this really nice to use!