WPILib Mecanum Drive, PID and drift

This is the first year that we are using C++ and also the first year for a mecanum drive. For ease of learning we are using the WPILib RobotDrive::Mecanum_Cartesian() with the gyro input. We have done pretty good reducing the drift of the gyro. However, when we try to drive straight forward, our robot drifts to the right. Mechanical has done quite a bit of work on the gear boxes and swapped motors and controllers, and the problem persists.

The thought has been to add a PID loop to lock on the angle, but we can not find a way to do this with the WPILib. I also can not find the source code for the library to see if this is possible, or even a really good description of how the code is actually supposed to work. We just don’t have the time at this point to write our own mecanum routines.

Has anyone ran into this problem and found any solutions? Also, does anyone know if the WPILib source code is available?

Source code can be found here (see the note on the project home page): https://usfirst.collab.net/sf/projects/wpilib/

One, often used, way of implementing a rotation PID using a gyro is to put it between the joystick and the rotation input of the RobotDrive mecanum method. Use the joystick to command changes in the angular setpoint and use the output of the PID as the rotation input of the mecanum drive method.

Do you mean like this? or like this? A gyro can be used to correct the second one, but the first one not so much.

Make sure you calibrate your motor controllers so that there is no unintended strafe or rotate command being given when you are trying to drive straight.

Try temporarily zeroing-out the strafe and rotate commands in software so that you can command a truly pure forward command and see if that affects the vehicle behavior. If it does, it may mean you are getting unintended strafe and/or rotate command when you don’t want it. If so, you could add deadband, or change the driver interface to separate the commands.


Using Mecanum_Cartesian() with the gyro input is intended to give you field-centric driving. The joystick commands are rotated to reflect the current heading of the robot. What would normally be “straight forward” becomes “down field”. If your robot drifts to the right, it means the gyro angle is measuring the robot as facing a little to the left of where it was facing when the gyro was calibrated on powerup.

If you don’t want field-centric motion control, don’t use the gyro input that way.

Thank you for the source code link!

As I understand the PIDController class, the output (Joystick class) must have a PIDOutput member or derived from that class. Joystick has neither. How would you implement that?

You could use the Gyro as the input for the PID loop, with Motors as the output, and use the Joystick to set the goal (setpoint) of the loop. It is kind of confusing because really each wheel would need its own PID controller, unless you create a PIDsubsystem which allows you to define what you do with the input and outputs.

My team also has drift when strafing and we’re trying to correct it with PID loops using encoders as the input, but selecting gains, etc is difficult expecially if you do not have extended periods of time with the robot to test (because other people are working on it).

Thanks to everyone for the help. We have some things to check out tomorrow.

Create a class implementing PIDOutput that does what you want.