Our team has decided to use mecanum drive for this years robot. When testing there has been a problem were our robot would drift when trying to strafe. To counteract this, I have been working on a P-Only control to correct the heading of the robot while driving. The code is not working well, as the drifting is still consistent when strafing, and in general drives worse than without the P-Only loop. I am not sure if this problem lies in my math for the power of each motor, if the constants for the P-Only loop are not tuned correctly, or if I am using the PID incorrectly. Any advise on how to fix this? Sample of code is below.
We did something similar last year, its probably not the right answer, but it worked well enough for us. Our 2022 drivetrain code should have what you’re looking for.
I was initially planning to use the MecanumDrive class, but it did not functioning properly. So instead I just made my own.
I can confirm it definitely works (barring any changes for this year). If youre interested in trying it again just to rule out the math errors in your own version, we can certainly help troubleshoot that.