Mecanum - use of gyro

[Jared Russell]

What you really ought to look into is PID (proportional, integral, derivative) control - there have been a ton of threads on ChiefDelphi over the years on the topic (search for "PID" and you should see them). All three of the FRC languages have facilities for making PID control fairly painless.

Right now what you have is essentially a proportional-only controller (you have feedback on the error between your desired yaw rate and your actual yaw rate) with a feed-forward element (your command gets added to the output) acting on yaw rate. As you observed, this works okay in certain situations (especially with a high P constant - k1 in your case). However, at some point, raising k1 will start to make your bot thrash from side to side as it oscillates around your desired angular rate - it is simply going too fast as it gets near its set point. The "I" and "D" parts of PID address this issue, as well as steady-state error (when your bot knows it needs to turn one way or another, but the proportional part isn't enough to get it to actually get there).

I would recommend using the accumulated gyro angle rather than the instantaneous yaw rate because usually it is yaw you actually want to control rather than yaw rate. Even more specifically, you usually want to maintain a CONSTANT yaw (keep pointing in the same direction) when you aren't moving the rotation joystick axis. Usually when you are using the rotation axis, you can get away without feedback control (the driver will take over in that case). In other words, I would suggest some logic like:

If Z is not close to zero
     Just apply Z to turning
Else if Z is close to zero
     Use PID to adjust Z to keep the gyro_angle at its current value
end