Swerve Rotation PID Problems

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

I have had this happen when the encoders are weird. The same thing happened to me on some recent modules with analog encoders.

I would plot the setpoint, position, and PID controller output of a single module that’s having the issue and see how it goes. Sometimes an encoder wrap can cause a problem.

Also, P-only controllers will naturally oscillate as you increase the kP. Add some kD to try and dampen it if the problem only occurs at high P.

1 Like

Check how noisy your encoder readings are.

Check how much delay there is in your measurements. Note that this might depend on your motor controller software, if you have (unwisely, imo) offboarded your control loops.

Make sure you use a feedforward that takes into account static friction so that small control outputs are not swallowed entirely (if your turning motors are weak neglecting to do this can completely ruin your loop performance).

1 Like

tell me more about why offboarding PID is not so amazing. I’ve heard that PID on motor controllers tends to be better as you don’t have the lag associated with taking in values from the controller over CAN and then sending values back to the motor controller

1 Like

The default filtering settings on most motor controllers introduce a significant amount of measurement lag (anywhere from ~20ms to ~100ms, depending on brand; you can look through the technical manuals if you like).

You can simply not plug your encoders into your motor controllers at all, and plug them into the DIO ports on the RIO. The difference in performance is going to be anything from a wash to a modest improvement over the above-mentioned defaults.

1 Like

A couple of other things to check:

-Is your PID controller code set to “continuous” rotation? (I.e., does it understand that -179 degrees to +179 degrees is a short distance?). There is usually an option on a PID controller to enable this, and you should enable it for swerve rotation.
-Where are you measuring the rotation? If it is directly measuring the wheel, that’s great. If it is measuring something connected to the wheel via a belt or gear, check to make sure the belt or gears aren’t occasionally skipping teeth

1 Like

Note that this is only true for velocity control. For position control, I wasn’t aware that the Falcon or Talon introduce filtering or delay. Maybe the Spark MAX does?
For such a simple system as swerve azimuth PID, I would be shocked if it made a difference. More likely, there is a software or tuning problem at work here.


We did see a noticeable amount of delay when using the cancoder as an azimuth feedback encoder… however it was more than good enough for anyone trying to get off the ground.

1 Like

I do not think the continuous mode is enabled, however the original swerve code I implemented is this one. This runs the loops off-board, however, I still do not see anything set for continuous input. Therefore I thought it wasn’t necessary, thanks for the advice.

Just added the necessary lines. Will try out and report back in a couple of days. Thanks!

Just had a chance to look more into the code.
What are these setup lines doing:
What are these angles representative of? I’m used to initializing the module using their cartesian coordinates.

This line is also suspect:
The “distance per rotation” should just be equal to the encoder ticks per rotation constant, if you want the getDistance method to return 4095 ticks per rotation (to match the precision of the encoder). Honestly, I’m not sure how this code is returning -180 to 180 as-is.

I think this is the source of your current problem though:
The resetAllEncoders() method will reset the rotation encoder, which sets its position to 0. You do not want to do this, because it will make your offsets wrong. If you only zero the drive encoder (which you should probably do in the teleop and autonomous init anyway) then you are good. Zeroing the rotation encoder will offset your modules by an unknown amount.


Thanks, the Rotation2Ds in the initialization are the rotation offsets for each module. We decided these by printing out the encoder values and then by manually turning each module to their approximate zero position.

About the .setDistancePerRotation() method, even though we set up the distancePerRotation, we do not use the encoder.getDistance() method to get data. Instead, we use encoder.get() to get raw data which I believe is between [-1, 1] for duty cycle encoders. So, this line is just bad maintained code. (Just commented it out.)

The actual code that converts raw data to the output is as follows:

  public double getDegrees(){
    return Math.IEEEremainder((rotEncoder.get() * 360.0 + offset.getDegrees()), 360.0);
    // ! Offset usage may be wrong

However, after your advice, I am also confused about my translation here (*360.). After your advice, I noticed that this code should output [0, 360] rather than [-179, 179]. However, I am pretty sure I did not see any values >180 on Glass. I will check this again to see where I am missing something.

For the last part, you are absolutely right. I am not sure if this is the only problem, but I will switch it to resetDriveEncoders().

Overall, I think you are right that there is something wrong with how the encoder values are manipulated. Thanks a lot for your help. Will look into these today on real hardware and report back.

1 Like

We have tried several things today, but we still have some issues. Firstly, our continuous mode is enabled and we are getting values of [-179, 179] from our encoders consistently and reliably.

When tuning the rotational PID, we go up until a certain kP value just fine. The wheels turn to their setpoints fine but in an extremely slow manner. Then, when kP is increased by a small amount (nowhere near where it should start oscillating) and some wheels start spinning back and forth like crazy. We are sure that the kP value is not the problem here because after a wheel starts spinning going back to an old kP value does not fix the issue unless it is really really small.

After a long time of observing, I have noticed something about the setpoints of the modules with this oscillation. Their setpoint goes back and forth in between ± 0.02 or less. This causes the sign of the setpoint to go from positive to negative back and forth. I am wondering if this could cause the error calculated by the PID controller to switch signs and therefore amplify the calculatedError causing such ridiculous turns. (I have tried to offset PID input and setpoint by 180 degrees to make the wheel angles [0, 360], but this did not seem to work either. However, I am not sure if I tried this offset method correctly.) (The encoder readings are still accurate btw.) I have tried to implement a deadband to accommodate (as seen below) for this issue but this did not show any significant change.

if (Math.abs(desiredRotation) <  0.2 ){
  desiredRotation = 0;

Here is a video of the wheels doing oscillatory motion. I am almost confident that this is not just because of high kP, because the last kP increment before the wheels go crazy, does a really slow turn towards the setpoint. I have thought of increasing the response time through implementing kI and then damping with kD. However, I believe that a regular kP loop should be just fine rather than going this route. Any guidance is really appreciated.

Have you plotted the setpoint, encoder position, and PID controller output yet?

1 Like

I have finally graphed the setpoint and module angles as you suggested. You can see the videos and the photos of graphs here.

As far as I understood, the PID setpoint goes back and forth 180 and -180, not understanding that it is the same angle. I am not sure what causes this. I have continuous mode enabled for the PID. I also tried to lower the min and max values (like -178, 178) for the continuous mode. I did not see any significant change by enabling/disabling or tweaking the continuous mode.

(As far as I can tell, all the encoder values are accurate.)
You can take a look at the code here. Any help is highly appreciated. Thanks!

Interesting, so it does look like the wrap isn’t working as expected. Can you write something that does the wraparound detection in code somehow? That is, can you convert a target of 179 and a sensor value of -177 to an error of +4 degrees?
I had to do something similar in my personal swerve code. You can find it here if you would like to take a look. It’s not as nice as yours but it is pretty short. https://github.com/asid61/Aliswerve2021

Note that my rotation sensor was a 3.3V analog encoder, so the raw sensor value was about 0-2700 counts when reading the 5V analog input.

1 Like

I noticed that you sent the continuous range to -178 to +178. Is there a reason why that’s not -180 to +180?

Also, I don’t quite understand this line of code in your swervemodule.Java:

( rotPID.calculate(
) +
rotFeedforward.calculate(rotPID.getSetpoint().velocity) ),

What does the rotFeedForward do there? I wouldn’t think you would have a feed forward for rotation like this, although your constants are zero so it’s probably doing nothing harmful.

Finally, you should check out this thread. It claims there is a bug In the PID controller’s class wraparound logic. I don’t know if that’s true, though: PID Continuous Mode Not wrapping - #4 by calcmogul

1 Like

This is the explanation of why the last value there was -178 and 178.

About the Feedforward side of things, I might be using it wrong but as far as I know, this would be the way to implement it. Any corrections would be appreciated.

About the last thread you provided, I looked over it and the issue seems to be resolved. Thanks a lot tho. I believe @calcmogul would know the best if this has anything to do with WPILib side of things. I highly doubt it as most teams have successfully implemented similar codes.

Lastly, I am planning to switch to a normal PIDController for testing purposes and see if ProfiledPIDController has anything to do with it. If not, I will have to write a wraparound code as @asid61 suggested. Are the wraparound code you have lines 83 and 84 in SwerveModule.java?

If there are any other suggestions or any other diagnoses of the problem, I would enjoy hearing it. Thanks!

The 2020-2021 ProfiledPIDController’s continuous mode support is broken (PIDController had the same bug). It’s fixed for 2022 (at least that’s what the unit tests imply).

1 Like

Probably. I also use the swerve kinematics optimize method to help out.