Issue with Configuring Swerve PID Gains for Turning

[NewDeanPlus]

Hello. I am the lead programmer for team 6512. This is our first year using swerve. We are using the SDS Mk4 modules with REV Neos and the Thrifty Absolute Encoders, and our code uses SparkMaxPIDController for PID calculation. In theory, the programming didn’t look too difficult, but we’ve been having a lot of trouble trying to figure out the PID gains and feedforward. We have looked at many guides and tutorials, but the PID constants seem to make our robot behave differently from how they are supposed to behave:

• using any P value causes the wheels to move directly to a value instead of oscillating like they’re supposed to
• wheels often turn around 60-90 degrees when the robot is enabled, even when they weren’t moved after enabling the robot for the last time (seems like the default position is changing?)
• Changing I or FF causes 1 or more wheels to oscillate violently, sometimes even to the point of blowing a fuse.
• Changing P after setting FF value causes it to do the opposite of what it is supposed to do (wheels oscillate violently below P value of about .2)

We have a small team of pretty inexperienced coders, and it feels like we’ve been getting nowhere over the past few weeks. We realized two of our encoders were broken for seemingly no reason at all a few weeks ago, so I feel like that could be an issue. I guess it could also be a code error, but I based the code off of a template for this kind of drivetrain and our mentor team recently helped us iron out some errors. I’m honestly not sure what it could be at this point. You can see our code here.

[truher]

I didn’t see anything obviously wrong with the code. i’d suggest that you should work up a new system like this from the bottom up: first validate that the sensors are sensing what you think they should be, by turning the axes by hand, and making some time series graphs in glass. does the angle wrapping work right? is the polarity right? then control the motors with the simplest possible input (i.e. set duty cycle). do they spin the way you expect? do the sensors agree with your eyes? does the onboard encoder agree with the absolute encoder? make lots of graphs to check. then make the motor control more sophisticated: if you want to use outboard closed loop, you should tune that from the bottom up: configure feed forward alone and drive in a straight line at a fixed speed, and measure the actual speed. adjust the ff to make it correct. measure the controller for error and make sure it’s small. repeat for the steering axes alone: turn at the correct speed. only when you have all eight axes configured and tuned correctly with simple test input should you try to actually use the “drivetrain” at all.

hope this helps!