Swerve Drive - Diagnosing an Issue

Our team received a set of SDS MK4’s with Falcon 500’s from a sponsor. I am playing with them before the season starts to see what I can do (emphasis on “play”, no intent to use them if I do not understand it). While we’ve been assembling and wiring the chassis, I’ve been working on the code (about 2 months in my off time). I followed the WPI SwerveCommand example and made the following code.

I wanted to ask anyone to take a quick look and see where I’m going wrong. I am attempting to learn this drive structure and I learn best by making something work then going through each line of code to understand what it is doing.

Right now, when enabled the robot violently shakes back and forth. The drive motors and turning motors both oscillate forward and backward. I have a feeling this may have something to do with my PID controller and I reduced kP to 0.1 to attempt a fix and I got the same issue.

I have also run characterization on the drive motors and they run. All devices, including the CANcoders, show up on Phoenix tuner and the snapshot test returned correct results.

So my question is, did I mess up the code? Do I have issues with my wiring? I hope someone can help me get started so I can begin fixing my own problem.


As a point of comparison with your code, I highly recommend reviewing or straight up using this code template provided by SDS. We had our teleop swerve up and running in under an hour using their example.

Their guide walks through the required changes including:

  • changing the team number
  • setting your offsets
  • we also had to reverse the direction of our joystick input

Out of curiosity would have adding 180 to all your encoder offsets worked. It seems if you want to do autonomous you would also have to be reversing that, and that would make multiple points of reversal. I am new to swerve and we are putting our kit together in 2 days. It is like Christmas.


Yes, this probably would have done the job as well!

I am actually going to use that template on the side, but the idea of this exercise was to learn how to generally program swerve drive. I’ve heard from others, like @Oblarg , that the supplied code is ambiguous. I could get the drive moving, but I won’t really understand why.

1 Like

If you have something that works it’s a lot easier to read the code and go “oh so this is why it works”.

Sounds like you might have an encoder wrapping issue. If you set your PID constants to 0, does the robot still shake?

I’ll try this out and report back!

Compare your PID parameters to the SDS example – that should at least get you in the ballpark and reduce the uncertainty. The symptoms you describe could be PID – pay a lot of attention to units.

I might just be missing it somewhere, but I do not see where you are using zeroing your cancoder values on each module.

I would recommend in the periodic function of your DriveSubsystem to do a smartdashboard printout of each absolute encoder value. See if it reads zero when you point it forward.

The cancoders have an absolute reading, but it will be dependent on the magnet orientation when you assembled it and will be a different value for each module.

Once you can see this reading. Use a long straight edge to line up the wheels and point them straight forward. Then record the absolute angle of each encoder.

Then you will need something like this in the constructer of each module.
this.m_turningEncoder = new CANCoder(turningEncoderPort);

where the angleZero is the absolute reading of the encoder without adding an offset.

Once you do this each encoder will read zero when pointing forward.

If I remember correctly WPI lib expects the module angle to be output in +180 to -180 degrees and you will have to make a method or adjust some settings to make the output in that format.

I don’t know if this is your full issue, but I think it is one you need to resolve. Once you get that resolved you will likely need to adjust the PID values.

My team is using NEOs so our code will not work directly for yours, but I heavily used the WPI lib example similar to yours so feel free to see what I have done.

I will give fair warning we are also in the experimenting phase of swerve and not long time veteran of swerve.

LHSPantherbots/2021SwerveDrive (github.com)

1 Like

Thanks for the reply. After getting the SDS MK4 code up an running on the bot, I was able to understand what this is used for. I put it in my code but I’m having the same issues. Looks like PID values are not the issue. The only thing left I can think of is issues with units somewhere. I’m going to run more prints to the dashboard and see if I can identify the isssue.

With the SDS MK4 code, I am having issues with the turning motors being twitchy every once in a while. I assume this is the behavior of a PID value being too high or the result of not using a trapezoid profile but that is only speculation based on my little understanding of it.

1 Like

Is the twitchyness with the turning or the driving or both. I found it helpful to go into the code isolate each. For example I would just multiply the drive output by zero and then the code would only try to turn the wheels, then switch this to isolate the drive. This helped me isolate where any twitch was coming from and if it was in the drive or turn. I have also found that the turn motors are much more twitchy on blocks than sitting on the ground. The weight of the robot and scrubbing friction dampens the dynamic response.

1 Like

I propped it up on blocks and it stopped twitching. kP too small?

It is hard for met to say without seeing it. Ours worked best when we added a feed forward to get it most of the way to the desired speed open loop then the PID had much less work to accomplish. If you are using the same code I don’t see any feed forward and I would recommend that on the drive motors. I didn’t find it was needed on the turn.

Most of the time that I have major twitching is when my P value is too high. When it is too low it usually just does not get to the required set point.

Also make sure you have a good battery. That is often a problem that can behave as you have described.

Update: First of all, I made the SDS MK4 program work. Turned out to be an electrical problem.

Second, I am working on making the WPI SwerveCommand example work. That is code I initially linked. I have got the robot moving but the motors are SUPER slow. Brought up kP all the way up to 0.6 before I realized something else has to be going on.

I ran into the same thing when I first set it up. I found that I had to multiply the joystick value by my max velocity. The default code seemed limit the max velocity to 1 m/s as that is what the max value the joystick output sends to the drive method.

I also found out the max turn rate for the modules was much too slow in the default wpilib code. I think I increases it 20x the default and it seemed to be much more controllable.

I actually noticed that and was not sure if the normalizeWheelSpeeds() method did that automatically. I have the top speed of my robot being 3 m/s (multiplying that by the controller input) but everything is still really slow. The PID outputs are topping out at 0.5 to 0.8 volts which seems really wrong. If I have everything in SI units, it should be outputting in volts, yea?

I’m going to print some more statements out today to see if I can find the issue and try to follow the logic closer to see where I’m messing up. Printing out the PID output helped me solve my issues so far.

It does not. All that does is ensure you don’t accidentally ask your drive to do something it is not physically capable of.

Okay good, I’ve been operating on that idea. After multiplying controller inputs by max speed it changed a lot.

I’m opening a PR to change the name of that method; this is the second time I’ve seen someone be confused.