Swerve MK4i Rotation Motor PID Oscillation

[yankee]

Hi everyone,

As 6838, this year we upgraded our mk4 swerve modules to mk4i with a conversion kit. Recently, we were trying out last season’s swerve code and had issues with the rotation motor’s PID algorithm. We are sure that we set the TalonFX and CANCoder IDs, and the Gear ratio correctly. After trying different kP values, the modules were oscillating almost randomly (sometimes two of the modules stayed still while the other two were oscillating). Hence, we decided to test the swerve code on a single module and worked with that code.

In that code, we tried to set the module angle at a constant setpoint and tuned the kP value according to that setpoint (i.e. 150 degrees). It seemed to work satisfactorily, however, once we set the setpoint to 40 degrees, the module was oscillating at a particular angle (far more different than the setpoint). Increasing the kP value ended up with an oscillation within almost 360 degrees while decreasing the kP value caused either to oscillate at a particular angle or stand still. Both tried a basic hand-written PID algorithm and the PIDController class and there was no change. We tried using both the signed and unsigned configurations for the CANCoder. However, nothing changed. The current CANCoder value was alternating between 170 and -170.

Here is a video of the constant setpoint trial. (The video is in slow-mo and a single PID algorithm is running throughout the whole video. The wheel turns to the desired rotation, then instantly rotates to the opposite direction, and then gets back to the desired one.)

Here is a video of a trial with a small kP value:

Here is another trial with a slightly greater kP value:

I believe that a proper PID output to the motor has to look like that graph.

However, according to my observation, our PID output looks like this:

In total, we tried using a constant setpoint, tuning kP values, using different CANCoder degree absolute sensor range configurations, a handwritten algorithm, and the PIDController class.

I appreciate it so much if anyone could help with this issue

Note: The optimization of the desired rotation angle was present in the first code we tried, however, we did not include it while working on a single module. Additionally, the offsets were not set as we left setting the offsets after solving the oscillation issue.

[prensing]

Did you flip the motors in the code? In going from MK4 to MK4i, I believe both motors need to be inverted from their settings as compared to how they were set before.

[Fletch1373]

I was about to ask about encoder direction, but your idea is much more likely given the information provided

[EyupAkgun]

@gdefender @Hollisms You have helped us a lot with this kind of issue before (thanks ). Maybe you can find a solution to this problem

[SLAB-Mr.Thomas]

The top image has odd behavior but the middle and bottom images look right - our robot does that sometimes in the air but on the carpet the wheels don’t overshoot their setpoint so much that it has to be corrected. We see on our motors from the motor controller lights that power is still being applied but so little that the motor doesn’t move once near it’s setpoint’s tolerance.

[sam.mulvey747]

Here is my guess:

If the steering motor has an output which is not in phase with the encoder (meaning positive voltage causes the motor to spin and the encoder goes down instead of up or the opposite) then the motor is going to accelerate away from the setpoint. Then, since the PID loop is circular, once it reaches the farthest point away from the setpoint it switches directions as the circular loop calculates a better way to get to the goal, but again the motor is going the wrong direction so it accelerates the wrong way. It keeps oscillating around the 180degree transform to your desired angle. This would look like the motor is oscillating around the correct desired setpoint because the wheel looks like it is facing the right direction still, but if you applied voltage to drive it would go the wrong direction. Just a guess but that’s my take from the videos

[gdefender]

Before we go too far troubleshooting this code, I want to make you aware of a couple things you might want to consider:

1. CTRE is releasing a swerve library for 2024. If all of your drivetrain hardware is CTRE (which it looks like yours is), you can use their Tuner X program to configure your drivetrain, and it generates a fully functional swerve project with reasonable PID defaults. You just need to adjust the values for the mass of your robot and you’re good to go. This is available now in beta, and it’s compatible with WPILib’s 2024 beta.
2. If you don’t want to use CTRE’s swerve library, I would still consider using the motor controller’s closed-loop position (or Motion Magic) control instead of running PID on the roboRIO. This will offload all of that work to the motor controllers. Plus the PID on the motor controller runs at 1kHz (1000x per second) vs. the roboRIO running at 50Hz (50x per second).

[Hollisms]

The 2nd and 3rd videos look normal to me if you are only using Kp. I would add Kd to prevent the overshoot.

This is my favorite video on PIDs.

Controlling Self Driving Cars

If adding Kd smooths it out just beware you’ll want to retune the system on the playing surface.

[sam.mulvey747]

One thing I want to note about the second and third videos is the motor does not seem to be slowing down as it approaches the “setpoint” which makes me think the direction is wrong

[yankee]

Definitely, that was the problem… Thanks, everyone.