We are trying to implement a swerve drive for the first time. I am aware that there have been overwhelmingly many swerve-related programming questions, but our team has been stuck and couldn’t figure a way out.
You can view our code here.
We are currently trying to tune the rotation PIDs. We are trying to do a simple P control for now.
However, as we are tuning the P, we see some problems.
When the P gain is very small, we can clearly see the wheels trying to turn to the setpoint. Also, when we try to rotate the robot, the wheels form a diamond shape. So, I believe that the kinematics and inverse kinematics are doing their jobs well. But the time it takes for them to turn to the setpoint is extremely slow.
However, as we increase P, we see a giant leap to a wild oscillatory motion. This motion is sometimes seen only on a single wheel and sometimes in all four of them. I believe that this is not caused by a very high kP value because when we go back to one of the smaller kP values, we still see fast oscillatory motion that does not settle down (encoder values still seem to be accurate without drift). Also, seeing this on a single wheel really confuses us, as other wheels continue to the setpoint. (We are also sure that this kP value is nowhere near the one we need as the wheels turn extremely slowly to the setpoint.)
We have checked the offsets and the encoder values. Our encoder range is [-180, 180] CW (looking at the robot from the bottom ) being positive. Sometimes, one of our wheel’s encoder values tends to slip -90 degrees and thinks that’s its zero point. The encoder status light is green is wired correctly and provides continuous data without any package drops or etc. We are not sure how to handle this and the tuning process of the rotation wheels. Even though this might cause one wheel to oscillate, we still have this throughout other modules.
Our joystick data also seems to be fine and does not show any systematic error.
The only thing I can think of is to have a conversion/unit, range error somewhere. Any tips on how to tune the rotation controllers for swerve drive is really appreciated. Thanks in advance