Swerve Drive Issues

Our software team is having an issue where whenever we run our drive motors in swerve drive using the velocity control mode, the wheels rotate back and forth constantly. We believe the issue is because of an issue with how we set our PID controller. Does anyone have any advice for how we could fix this issue or next steps? We are using 4 motors for driving, and 4 for turning the wheels. We are using TalonFX motors. We have the P value currently set to 0.5 for both the turning and drive motors. All the code we are using is in Main.java, Robot.java and under subsystems SwerveDrive.java and SwerveModule.java. Here is a link to the code. Thanks!

So the issue is your modules rotate back and forth very quickly? Lets take a look:

Looking at your code, your logic for converting an angle to the position of your module looks like this.


If we wanted to move to 90 degrees, we’d get something like

6,553.6 = 2048/(2*pi)*(pi/2)*12.8

Your position setpoint is 6553. If you were at 0 degrees currently, then your current position would be 0. So the immediate error would be 6553. Multiplied by your P value in the PID controller would result in a motor output percent of

3276 = 6553 *.5

Your PID controller is sending a ridiculously high value. You want that value to be closer to 1,-1. Going over to like 10, -10 is fine, but going to 3000 is overkill. To show that, lets say you are at 89 degrees and you want to go to 90 degrees, your error is 1 degree, lets convert that to encoder ticks using your formula again

72.8 = 2048/(2*pi)*(0.0174533)*12.8

So 1 degree of error translates to basically 73 encoder ticks. 73 encoder ticks multiplied by your P value of .5 is

36.5 = 73*.5

Your P is way too high. At 1 degree of error, it is very generously sending 100% speed. What is a reasonable P value? I do this by deciding what the minimum error we want the controller to move at 100% speed to get to. So if we said we wanted to move full speed when we were 90 degrees off, we can use your math to solve for P

Motor Output = 2048/(2*Math.PI)*state.angle.getRadians()*kTurning*kP
1 = 2048/(2*pi)*(pi/2)*12.8*kp
1 = 6553.6*kp
kp = 1/6553.6
kp = .00015

From there, I tweak that P value after watching it perform. If it’s too slow, try doubling it. If it’s WAY too slow, multiply by 10. Too fast, do the opposite.


Hey thanks for the reply! I appreciate the insight on our turning motor values, but our current issue with the robot relies more on the actual drive motors than the rotation/angle motors. The drive motors are the ones that keep spinning back and forth. We tried messing with the kP values on them at kP = 0.294 and kP = 0.128, however, they had the same issue and when testing with them they didn’t have enough force to move the robot. Our issue lies more with line 152 of SwerveModule.java. Thanks!

Sure sure. Well you likely have a similar issue with having too high of a PID value since your units are “ticks/100ms”.

If you’re velocity is just 10 ticks off, you’re going to send a value of 100% speed using the same math, with that pid. I would dial your P value down significantly. Like to .001 and then see what happens.

Good chance that when you’re using a PID controller and have oscillation, your P is too high.


This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.