Mecanum - use of gyro

[mwsmith78]

Our team is in the process of playing around with a prototype mecanum chassis. We have a gyro installed and are trying to use it to correct for rotation errors (to drive straight).

Currently we are using the raw gyro rate (g) and the input from joystick (z) to feed the following logic ...

z' = k1 * (z - (g * k2)) + z

K1 is a P constant
K2 is factor to convert deg/sec to something in the -1 to 1 ballpark

It works, but not as smooth as we would like. We still get some drifting, but other times you can see it correcting (especially when we inflate k1).

Are we on the right track with out logic? do we just need to tweak constants more?
Do we need to filter the gyro input?
Anyone want to offer up some sample Labview code?

Related to the error correction, the team saw Team 1058's awesome mecanum video - where the robot corrects its rotation after impact. Does that (high freq) need to be handled differently that rotation drift error (low freq)?

Should we build our logic using the accumulated Gyro Angle variable? have the error correction work off the difference in the desired angle and the gyro angle? Seems like using the Angle values would cover both the Drift and Impact scenarios. But, how do you go about accumulating a desired angle from the joystick inputs? you would need to know the sampling time of joystick.

Any help would be much appreciated

Thanks