I recommend using one of the WPIlib MecanumDrive methods, and just make corrections to the "twist" input when you want to go straight based on the error. Do not use the fourth "gyroscope" input to MecanumDrive_cartesian; this is only used for "field coordinates".
The correction could be a full PID, but because of the amount of friction in a mecanum drive system, you should be able to get away with a simple proportional correction; D will be mechanical, and I is usually not needed when seeking a position.
To do this, you will need to create a numeric variable to hold the desired heading, say azi.
In your joystick-reading loop:
- When the joystick values indicate that you do not wish to hold a constant facing, set azi to NaN, or some number outside of the valid range.
- When the joystick values indicate that you do wish to hold a constant facing:
- If isnan(azi), set azi =gyro.getAngle()
- otherwise, add a correction of the form prop * (azi - gyro.getAngle()) to the "twist" value. You will have to tune the value of prop.
Debugging tip: If the robot starts spinning faster and faster, do a quick shut down, change the sign of "prop" and try again.