SDS MK4i Swerve Drive Issues

I am having some issues with programming swerve for my team. We are using sds MK4i modules with falcons for drive and Neos for steer. Everything seems so work fine when it’s off the ground but as soon as we try to drive it problems occur. Some of the drive motors don’t spin but the steer motors work as intended. I don’t believe they are fighting each other as all when it’s off the ground they all spin in one direction.

Here’s the code I have so far …

So a quick look at your codebase doesn’t reveal an obvious issue. This isn’t to say there isn’t one or more, but it just means I don’t have a “silver bullet” answer for you.

General advice for how to start to break down the problem of “swerve doesn’t work”:

  1. Work from inputs to outputs
  2. Pick a small subset of functionality to check out
  3. Use things like SmartDashboard.putNumber("name of value", value); to inspect how numbers inside your codebase change over time.
  4. Think like a scientist. Make a hypothesis about how a certain piece of code should behave in a certain scenario. Add enough debug info to allow you to prove or disprove your hypothesis. Perform an experiment on your robot, capturing data. Then analyze the data to determine if your hypothesis
  5. Work methodically. Break your code, at least mentally, into chunks of “input, processing, output”. If inputs are “what you expect” (working), but outputs are “not what you expect” (broken), it would imply the processing between them is broken. If inputs are broken, move your debug efforts to the things which drive those inputs. If outputs are correct, move downstream to the next steps to find something that is broken.

Print logs for your output drive speeds. See if there are reasonable number (like greater than .1 and less than 1)

I can’t tell for sure, but you may have forgotten parenthesis along the way.

    public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 / 60.0 * 
         SdsModuleConfigurations.MK4_L2.getDriveReduction() * 
         SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI;