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!
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.
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).