MK4 Swerve drive problems! Need help!

Hi Everyone, I was wondering if you guys could help us. We have been slamming our heads against the wall trying to solve this problem. We bought the sds MK4 swerve drive modules. We had them working two weeks ago for our protobot and just finished reassembling the final robot. We added a ton of code but I believe none of it is impacting our main drive subsystem. When we started up our robot it drives properly for about 10-30 seconds and then the controls just get really weird. The robot starts spinning when we want it to go straight, etc. I was wondering if anyone could take a look at my students code and see if there is anything that looks wrong or that might be causing the problem? Here is the github: https://github.com/BizarbotsRobotics/2022RobotCode Thanks!!

Hey, try disabling all other commands, and then driving around. (Just comment them out) and see what happens. I can’t tell what is going on without extra information like that.

This is likely the reset code the sds library uses. See the other discussions on it.

EDIT: Jason’s answer is what to look into first. My suggestions below apply if that does not pan out.

Hmmm. The symptom described unfortunately doesn’t have a single root cause, so it’s going to be hard to pinpoint one or more issues in a codebase (plus an unknown electrical/mechanical system) without more info.

A few pieces of debugging info to help narrow the scope of the search:

  1. Using some dashboard, can you generate plots of the following:
    1a) Actual & Desired azimuth angle, and applied motor voltage for all swerve modules
    1b) Actual & Desired wheel speed, and applied motor voltage for all swerve modules
    1c) Gyroscope measured angle and driver rotation command

In each plot, ideally, mark out which sections reflect “robot working as expected”, and “robot not working as expected”

  1. How is your CAN bus load looking, in %? High/low? Consistent and steady, or regularly jumping around?

  2. any errors or warnings showing up in the rioLog?

if you are using the mxp slot for the imu and if the bot browns out even briefly the rio will cut power to your imu. verify the rio is not browning out from current draw

I’ll try one in a later today.

Here is the graph of our can utilization:

I believe we sometimes get lost CAN frames when we are pulling too much current

We are using can for our IMU