Auto trimming using the gyro

So this summer I have decided to work on programming challenges that would make our team more competitive during the FIRST season but that wouldn’t necessarily have time to do during the first season.

One problem I’m trying to tackle is how can I use the gyro deg/sec output to calibrate a realtime auto trim for out robot. We have a mechanum system btw. I think I really only need to trim the z axis (rotation) so the accelerometer wouldn’t be used. I need to figure out the correct formula to modify the z joystick value based on the gyro input and the current z axis joystick input (as if I were turning).

Does anyone have any ideas as to how to do this?

what I hear you saying is this:

a) you want the vehicle to rotate at a speed proportional to the z-axis command when the z axis command is non-zero, and

b) when the z-axis command is zero, you want to use the gyro to hold the vehicle’s rotational orientation fixed regardless of X and Y motion.

is that correct? If not, please explain in more detail what you are trying to accomplish.


The basic idea is pretty simple. Use the yaw rate sensor to derive a “current heading” measurement value; the gyro support in the library does that for you. Maintain a “desired heading” control value, using joystick input to change the desired value. Tie the two of them together as the feedback and control parameters of a PID system, using the output of the PID as the “rotation” input of the mecanum drive.

I hope enough of that makes sense to give you a place to start.

You could try the following simple approach, which is similar to what Team 341 (Miss Daisy) used successfully:

if (Z!=0) Z'=Z else Z'=-K*gyro_rate;

Z is the joystick value from its “rotate” command axis

Z’ is the modified value of Z

gyro_rate is the rotation speed reading from the gyro

K is a tuning constant, which you can adjust experimentally.

what the code does is this:

a) when you are commanding rotation (Z is not zero), your rotate command (Z) is passed through unmodified as an open-loop command

b) when you are commanding zero rotation (ie Z is zero), the gyro’s rotation signal is used instead of Z=0 to create a Z command to cancel out the rotation.

A slightly different approach might be:

Z' = K*(Z-gyro_rate);

… which is a “P” (proportional) controller which uses your joystick Z value and the gyro signal to create the modified Z’ command. This controller acts to make the robot’s sensed rotational velocity match the joystick command regardless of whether it is zero or non-zero.